#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/static_transform_broadcaster.h"

template <typename T, typename t1, typename t2, typename t3>
void xyz_set(T &obj, t1 x, t2 y, t3 z)
{
	obj.x = x;
	obj.y = y;
	obj.z = z;
}

class speed_const : public rclcpp::Node
{
  private:
	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_pub_static;
	std::shared_ptr<tf2_ros::TransformBroadcaster> tf_pub;
	std::shared_ptr<tf2_ros::TransformListener> tf_sub;
	std::shared_ptr<tf2_ros::Buffer> tf_buf;
	rclcpp::TimerBase::SharedPtr timer;

	void timer_callback(void)
	{
		static double time = 0;
		geometry_msgs::msg::TransformStamped tf;

		tf.header.frame_id = "map";
		tf.header.stamp = rclcpp::Clock().now();
		tf.child_frame_id = "base";
		xyz_set(tf.transform.translation, sin(time), 0, 0);
		this->tf_pub->sendTransform(tf);

		tf.header.frame_id = "cam";
		tf.header.stamp = rclcpp::Clock().now();
		tf.child_frame_id = "obj";
		xyz_set(tf.transform.translation, 1, sin(time), 1);
		this->tf_pub->sendTransform(tf);

		
		geometry_msgs::msg::PoseStamped point;
		point.header.frame_id = "laser";
		xyz_set(point.pose.position, 10, 10, 10);
		auto pout = this->tf_buf->transform(point, "obj", tf2::durationFromSec(5));
		RCLCPP_INFO(this->get_logger(), "point in obj axis: %lf, %lf, %lf",
					pout.pose.position.x, pout.pose.position.y, pout.pose.position.z);
					
		time += 0.1;
	}

  public:
	speed_const(std::string name) : Node(name)
	{

		this->tf_pub = std::make_shared<tf2_ros::TransformBroadcaster>(this);
		this->tf_buf = std::make_shared<tf2_ros::Buffer>(this->get_clock());
		this->tf_sub = std::make_shared<tf2_ros::TransformListener>(*this->tf_buf);
		
		this->tf_pub_static = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

		geometry_msgs::msg::TransformStamped tf;
		tf.header.frame_id = "base";
		tf.header.stamp = rclcpp::Clock().now();
		tf.child_frame_id = "cam";
		xyz_set(tf.transform.translation, 0.1, 0, 0);
		this->tf_pub_static->sendTransform(tf);

		tf.child_frame_id = "laser";
		xyz_set(tf.transform.translation, 0, 0, 1);
		this->tf_pub_static->sendTransform(tf);


		tf.header.frame_id = "map";
		tf.header.stamp = rclcpp::Clock().now();
		tf.child_frame_id = "base";
		xyz_set(tf.transform.translation, 1, 0, 0);
		this->tf_pub->sendTransform(tf);

		tf.header.frame_id = "cam";
		tf.header.stamp = rclcpp::Clock().now();
		tf.child_frame_id = "obj";
		xyz_set(tf.transform.translation, 1, 1, 1);
		this->tf_pub->sendTransform(tf);

		this->timer = this->create_wall_timer(std::chrono::milliseconds(100),
											  std::bind(&speed_const::timer_callback, this));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<speed_const>("speed_const");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
